#include "main.h"

volatile int keep_running = 1;

static const uint8_t CRC16HiTable[]=
{ 
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40   
};

//CRC16校验低位字节表
static const uint8_t CRC16LoTable[]=
{
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
    0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
    0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
    0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
    0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
    0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
    0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
    0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
    0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
    0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
    0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
    0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
    0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
    0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
    0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
    0x43, 0x83, 0x41, 0x81, 0x80, 0x40     
};

static uint16_t CRC_Compute(uint8_t *buf,uint16_t len)
{
    uint8_t index;
    uint16_t check16=0;
    uint8_t crc_low=0XFF;
    uint8_t crc_high=0XFF;
    while(len--)
    {
        index = crc_low^(*buf++);
        crc_low  = crc_high^CRC16HiTable[index];
        crc_high = CRC16LoTable[index];
    }
    check16 += crc_high;
    check16 <<= 8;
    check16+=crc_low;
    return check16;
}

static int setup_can_socket(const char *ifname) 
{
    int s;
    struct sockaddr_can addr;
    struct ifreq ifr;
    int ret;
    char cmdBuf[128];

    snprintf(cmdBuf,sizeof(cmdBuf),"ifconfig %s down",ifname);
    ret = system(cmdBuf);
    snprintf(cmdBuf,sizeof(cmdBuf),"canconfig %s stop",ifname);
    ret = system(cmdBuf);

    snprintf(cmdBuf,sizeof(cmdBuf),"canconfig %s bitrate 500000 ",ifname);
    ret = system(cmdBuf);

    snprintf(cmdBuf,sizeof(cmdBuf),"canconfig %s start",ifname);
    ret = system(cmdBuf);

    if (-1 == ret)
    {
        printf("canconfig system error!");
        return -1;
    }

    if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) 
    {
        perror("socket");
        return -1;
    }

    strcpy(ifr.ifr_name, ifname);
    ioctl(s, SIOCGIFINDEX, &ifr);

    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) 
    {
        perror("bind");
        close(s);
        return -1;
    }

    printf("[INFO] Bound to interface: %s\n", ifname);
    return s;
}

static void send_response(int can_fd, uint8_t *recvBuf) 
{
    int     i = 0;
    uint8_t sendBuf[1024];
    int     sendLen = 0;
    uint8_t *pData = NULL;
    
    uint8_t chn = recvBuf[1];  // 第一个字节是长度 第二字节是通道号

    for(i = 0; i < ARRAY_SIZE(msgAddrMap); ++i)
    {
        if(msgAddrMap[i].chn == chn && 
           msgAddrMap[i].devAddr == recvBuf[2] && 
           msgAddrMap[i].funCode == recvBuf[3] &&
           msgAddrMap[i].regAddr == recvBuf[4]*256 + recvBuf[5])
        {
#if DEBUG           
            printf("desc=%s chn=%d devAddr=%d funCode =%d regAddr=%04X\n",
            msgAddrMap[i].desc,msgAddrMap[i].chn,msgAddrMap[i].devAddr,
            msgAddrMap[i].funCode,msgAddrMap[i].regAddr);
#endif
            switch(recvBuf[3])
            {
                case 3:
                case 1:
                case 2:

                    pData = (uint8_t *)getDataAddrByAddr(msgAddrMap[i].addr);
                    if(pData == NULL)
                    {
                        return;
                    }

                    sendLen = 5;

                    for(int j = 0; j < msgAddrMap[i].dataNum; j++)
                    {
                        if(msgAddrMap[i].dataTypeTab[j] == U16)
                        {
                            sendBuf[sendLen++] = (*(pData+1)) & 0xFF;
                            sendBuf[sendLen++] = (*pData) & 0xFF;
                            pData += 2;
                        }
                        else if(msgAddrMap[i].dataTypeTab[j] == U32)
                        {
                            sendBuf[sendLen++] = (*(pData+3)) & 0xFF;
                            sendBuf[sendLen++] = (*(pData+2)) & 0xFF;
                            sendBuf[sendLen++] = (*(pData+1)) & 0xFF;
                            sendBuf[sendLen++] = (*pData) & 0xFF;
                            pData += 4;
                        }
                        else if(msgAddrMap[i].dataTypeTab[j] == U8)
                        {
                            sendBuf[sendLen++] = (*pData) & 0xFF;
                            pData++;
                        }
                        else
                        {
                            printf("unsupport data type!\n");
                            return;
                        }
                    }

                    sendLen += 2;
                    sendBuf[0] = sendLen;
                    sendBuf[1] = chn;
                    sendBuf[2] = msgAddrMap[i].devAddr;
                    sendBuf[3] = msgAddrMap[i].funCode;
                    sendBuf[4] = sendLen - 5 - 2;
                    break; 
                case 5:
                case 6:
                    if(msgAddrMap[i].opType == T_CtrlStatus)
                    {
                        //静态：0x7777  开机：0xDDDD 关机：0xEEEE
                        if(recvBuf[6] == 0x00 && recvBuf[7] == 0x00)
                        {
                            setDataParam(msgAddrMap[i].addr,0xEEEE);
                        }
                        else
                        {
                            setDataParam(msgAddrMap[i].addr,0xDDDD);
                        }
                    }
                    else if(msgAddrMap[i].opType == T_CtrlSwicth)
                    {
                        if(recvBuf[6] == 0x00 && recvBuf[7] == 0x00)
                        {
                            setBit(msgAddrMap[i].addr/10,DO_TYPE,msgAddrMap[i].addr%10,0);
                        }
                        else
                        {
                            setBit(msgAddrMap[i].addr/10,DO_TYPE,msgAddrMap[i].addr%10,1); 
                        }
                    }
                    else if(msgAddrMap[i].opType == T_SetValue)
                    {
                        uint16_t value = recvBuf[7] << 8 | recvBuf[6];
                        setDataParam(msgAddrMap[i].addr,value);
                    }
                    memcpy(&sendBuf[2],&recvBuf[2],8);
                    sendLen = 10;
                    sendBuf[0] = sendLen;
                    sendBuf[1] = chn;
                    break;
                default:
                    break;
            }
            break;
        }
    }

    if(i >= ARRAY_SIZE(msgAddrMap) || sendLen < 4)
    {
        return;
    }

    uint16_t crc = CRC_Compute(&sendBuf[2],sendLen-2-2);
    sendBuf[sendLen-2] = crc & 0xFF;
    sendBuf[sendLen-1] = (crc >> 8) & 0xFF;

#if DEBUG
    printf("[INFO] Send CAN message: ");
    for (i = 0; i < sendLen; ++i)
    {
        printf("%02X ", sendBuf[i]); 
    }
    printf("\n");
#endif

    int num_frames = (sendLen + 6) / 7;

    for(i = 0; i < num_frames; ++i) 
    {
        struct can_frame frame;
        frame.can_id = CAN_ID; // 设置适当的CAN ID
        frame.can_dlc = (sendLen - i * 7) > 7 ? 7 : (sendLen - i * 7);

        memcpy(&frame.data[1], &sendBuf[i * 7], frame.can_dlc);
        
        frame.data[0] = i; //序列号

        frame.can_dlc += 1;
        
        // 发送CAN帧
        write(can_fd, &frame, sizeof(struct can_frame));
    }
}

void signal_handler(int sig) 
{
    printf("Received signal %d, performing cleanup...\n", sig);
    keep_running = 0;
}

int main(int argc, char *argv[]) 
{
    uint8_t recvBuf[1024];
    int     len = 0;
    int     recvLen = 0;
    struct can_frame frame;
    const char *ifname = "can0";

    signal(SIGINT,  signal_handler);
    signal(SIGTERM, signal_handler); 

    syslog(LOG_DEBUG,"data_can_modbus 程序版本日期：%s %s",__DATE__,__TIME__);

    int can_fd = setup_can_socket(ifname);

    if (can_fd < 0) 
    {
        fprintf(stderr, "[ERROR] Failed to open CAN interface\n");
        exit(EXIT_FAILURE);
    }

    printf("[INFO] Listening on CAN interface...\n");

    if (shareMemInit(&mapCfg) < 0)
    {
        fprintf(stderr, "shareMemInit failed\r\n");
        goto exit;
    }

    struct timeval timeout;
    timeout.tv_sec = 1;       // 秒
    timeout.tv_usec = 0;      // 微秒

    setsockopt(can_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));

    while (keep_running)
    {
        ssize_t nbytes = read(can_fd, &frame, sizeof(struct can_frame));

        if (nbytes > 0 && (frame.can_id & CAN_EFF_MASK) == CAN_ID) 
        {
            uint8_t seq_num = frame.data[0];

            if(len == 0 && recvLen == 0)
            {
                if (seq_num != 0)
                {
                    continue;   //认为第一包的序号一定是0，如果不是就全部丢弃，直到接受到序号为0的包
                }
                else
                {
                    len = frame.data[1];
                }
            }

            seq_num = seq_num > 70 ? 70 : seq_num; //防止越界

            memcpy(recvBuf + seq_num * 7, frame.data + 1, frame.can_dlc - 1);

            recvLen += frame.can_dlc - 1;

            if(len != 0 && recvLen >= len)
            {
#if DEBUG                
                printf("[INFO] Received CAN message: ");
                for (int i = 0; i < len; ++i) 
                {
                    printf("%02X ", recvBuf[i]);
                }
                printf("\n");             
#endif
                send_response(can_fd, recvBuf);

                len     = 0;
                recvLen = 0;
            }
        }
    }

exit:
    close(can_fd);
    unmapMem(&mapCfg);
    return 0;
}